﻿#include <gtest/gtest.h>

#include "geom/geom_curve_bezier.h"
#include "geom/geom_curve_bspline.h"
#include "kernel/spline.h"

#include <algorithm>
#include <iostream>

#include <dbg.h>

using namespace CAGD;

// 测试样条函数
TEST(TestSpline, FindSpan)
{
    {
        std::vector<double> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});
        EXPECT_TRUE(Spline::FindSpan(0.0, 2, knots) == 2);
        EXPECT_TRUE(Spline::FindSpan(0.5, 2, knots) == 2);
        EXPECT_TRUE(Spline::FindSpan(1.5, 2, knots) == 3);
        EXPECT_TRUE(Spline::FindSpan(2.5, 2, knots) == 4);
        EXPECT_TRUE(Spline::FindSpan(3.5, 2, knots) == 5);
        EXPECT_TRUE(Spline::FindSpan(4.5, 2, knots) == 7);
        EXPECT_TRUE(Spline::FindSpan(5.0, 2, knots) == 7);
    }

    {
        std::vector<double> knots({0, 0, 0, 1, 2, 2, 4, 4, 5, 5, 5});
        EXPECT_TRUE(Spline::FindSpan(0.0, 2, knots) == 2);
        EXPECT_TRUE(Spline::FindSpan(0.5, 2, knots) == 2);
        EXPECT_TRUE(Spline::FindSpan(1.5, 2, knots) == 3);
        EXPECT_TRUE(Spline::FindSpan(2.5, 2, knots) == 5);
        EXPECT_TRUE(Spline::FindSpan(3.5, 2, knots) == 5);
        EXPECT_TRUE(Spline::FindSpan(4.5, 2, knots) == 7);
        EXPECT_TRUE(Spline::FindSpan(5.0, 2, knots) == 7);
    }

    {
        std::vector<double> knots({0, 0, 0, 1, 2, 4, 4, 4, 5, 5, 5});
        int k = Spline::FindSpan(2.5, 2, knots);
        EXPECT_TRUE(Spline::FindMult(k, 2.5, 2, knots) == 0);
        k = Spline::FindSpan(2.0, 2, knots);
        EXPECT_TRUE(Spline::FindMult(k, 2.0, 2, knots) == 1);
        k = Spline::FindSpan(4.0, 2, knots);
        EXPECT_TRUE(Spline::FindMult(k, 4.0, 2, knots) == 3);
        k = Spline::FindSpan(0.0, 2, knots);
        EXPECT_TRUE(Spline::FindMult(k, 0.0, 2, knots) == 1);
    }
}

// 测试样条函数
TEST(TestSpline, BasisFuncs)
{
    {
        std::vector<double> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});
        std::vector<double> N = Spline::Basis(4, 2.5, 2, knots);
        EXPECT_TRUE(N == std::vector<double>({1.0 / 8, 6.0 / 8, 1.0 / 8}));
    }

    {
        std::vector<double> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});
        std::vector<std::vector<double>> N(3);
        N[0] = std::vector<double>({1});
        N[1] = std::vector<double>({0.5, 0.5});
        N[2] = std::vector<double>({1.0 / 8, 6.0 / 8, 1.0 / 8});
        EXPECT_TRUE(Spline::DerivAllBasis(4, 2.5, 2, knots) == N);
    }

    {
        std::vector<double> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});
        int span = Spline::FindSpan(2.5, 2, knots);
        EXPECT_TRUE(span == 4);
        EXPECT_TRUE(Spline::DerivBasis(2, span, 2.5, 2, knots) ==
                    std::vector<std::vector<double>>({{0.125, 0.75, 0.125}, {-0.5, 0.0, 0.5}, {1.0, -2.0, 1.0}}));
    }
}

// 测试样条曲线
TEST(TestSpline, Curve)
{
    {
        std::vector<BPnt> points(8);
        for (int i = 0; i < 8; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        std::vector<double> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

        GeomBSplineCurve spline(knots, points);
        EXPECT_TRUE(spline.Value(2.5) == (points[2] / 8 + points[3] * 6 / 8 + points[4] / 8));
    }

    {
        int m = 7;
        int p = 2;
        int n = m - p - 1;
        std::vector<BPnt> points(5);
        for (int i = 0; i < 5; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        std::vector<double> knots({0, 0, 0, 0.4, 0.6, 1, 1, 1});

        GeomBSplineCurve spline(knots, points);
        // 求导后只有 4 个有效的控制点，剩下的为零（空）
        std::vector<BPnt> derivPoints(5);
        derivPoints[0] = (points[1] - points[0]) * 5;
        derivPoints[1] = (points[2] - points[1]) * 10 / 3;
        derivPoints[2] = (points[3] - points[2]) * 10 / 3;
        derivPoints[3] = (points[4] - points[3]) * 5;
        std::vector<std::vector<BPnt>> PK = spline.DerivCpts(1, 0, n);
        EXPECT_TRUE(PK[0] == points);
        EXPECT_TRUE(PK[1] == derivPoints);
    }

    {
        int m = 10;
        int p = 3;
        int n = m - p - 1;
        std::vector<BPnt> points(7);
        for (int i = 0; i < 7; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        std::vector<double> knots({0, 0, 0, 0, 1.0 / 3, 2.0 / 3, 2.0 / 3, 1, 1, 1, 1});

        GeomBSplineCurve spline(knots, points);

        // 求导后只有 6 个有效的控制点，剩下的为零（空）
        std::vector<BPnt> derivPoints(7);
        derivPoints[0] = (points[1] - points[0]) * 9;
        derivPoints[1] = (points[2] - points[1]) * 4.5;
        derivPoints[2] = (points[3] - points[2]) * 4.5;
        derivPoints[3] = (points[4] - points[3]) * 4.5;
        derivPoints[4] = (points[5] - points[4]) * 9;
        derivPoints[5] = (points[6] - points[5]) * 9;
        std::vector<std::vector<BPnt>> PK = spline.DerivCpts(1, 0, n);
        EXPECT_TRUE(PK[0] == points);
        EXPECT_TRUE(PK[1] == derivPoints);
    }

    {
        int m = 10;
        int p = 2;
        int n = m - p - 1;
        std::vector<BPnt> points(8);
        for (int i = 0; i < 8; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        std::vector<double> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

        GeomBSplineCurve spline(knots, points);
        std::vector<BPnt> CK = spline.DerivPoints(1, 2.5);
        EXPECT_TRUE(CK[1] == (points[2] * -0.5 + points[4] * 0.5));
    }

    {
        auto f = [](float t) -> BPnt { return BPnt{0.0005 * t * t, 5 * sin(t), 0.05 * t}; };

        std::vector<BPnt> points5(9);
        for (int i = 0; i < 9; i++)
            points5[i] = f(10 * i);
        std::vector<double> knots2({-2, -1, 0, 0.8, 1, 2, 3, 4, 4.5, 5, 6, 7, 8});
        GeomBSplineCurve *spline = new GeomBSplineCurve(knots2, points5);
        spline->InsertKnot(2.5, 2);

        GeomBSplineCurve *spline2 = new GeomBSplineCurve(knots2, points5);
        double a = spline2->FirstParameter();
        double b = spline2->LastParameter();
        double step = (b - a) / 999;
        for (int i = 0; i < 1000; i++)
        {
            double u = a + step * i;
            EXPECT_TRUE(spline->Value(u) == spline2->Value(u));
        }
    }

    {
        auto f = [](float t) -> BPnt { return BPnt{0.0005 * t * t, 5 * sin(t), 0.05 * t}; };

        std::vector<BPnt> points4(8);
        for (int i = 0; i < 8; i++)
        {
            points4[i] = f(10 * i);
            points4[i][0] *= 5;
            points4[i][0] -= 5;
        }
        std::vector<double> knots({-1, 0, 0.5, 1, 2, 3, 4.2, 4.5, 5, 6, 7});

        std::vector<BPnt> points5(9);
        for (int i = 0; i < 9; i++)
        {
            points5[i] = f(10 * i);
            points5[i][0] *= 5;
            points5[i][0] -= 5;
            points5[i][0] -= 2;
        }
        std::vector<double> knots2({-2, -1, 0, 0.8, 1, 2, 3, 4, 4.5, 5, 6, 7, 8});

        GeomBSplineCurve *spline = new GeomBSplineCurve(knots2, points5);
        spline->RefineKnot({0.9, 1.6, 2.5, 4.3, 4.3, 4.6});

        GeomBSplineCurve *spline2 = new GeomBSplineCurve(knots2, points5);
        double a = spline2->FirstParameter();
        double b = spline2->LastParameter();
        double step = (b - a) / 999;
        for (int i = 0; i < 1000; i++)
        {
            double u = a + step * i;
            EXPECT_TRUE(spline->Value(u) == spline2->Value(u));
        }
    }
}